#include "bsp/cpu_ticks.h"
#include "controller/motor.h"
#include "utils/utils.h"
#include "utils/logger.h"
#include "os/os.h"

motor_t g_motor[1];

static void motor_force_timeout(os_timer_t *timer);

void motor_init(motor_t *motor){
    memset(motor, 0, sizeof(motor_t));
	memset(motor->plot_ids, 0xFF, PLOT_MAX_IDS);
    mc_conf_init();
	os_timer_init(&motor->force_timer, motor_force_timeout);
	motor_init_encoder(motor);
	pwm_timer_init(mc_conf()->pwm_freq);
	hw_sample_init(&motor->samples);
	contrl_init(controller(motor));
    mc_sched_timer_init(freq_2_us(CONFIG_SPEED_CTRL_FREQ));
	hw_sample_get_phacurr_offset(&motor->samples);
	adc_start_convert();
	sys_debug("motor_init\n");
}

bool motor_start(motor_t *motor, u8 mode) {
	if (motor->b_start) {
        return true;
    }
	sys_debug("motor start\n");
	if (board_get_error()) {
		sys_debug("board have same errors\n");
		return false;
	}
	u32 mask = cpu_enter_critical();
	contrl_start(controller(motor), mode);
	motor->b_start = true;
	cpu_exit_critical(mask);
	pwm_start_output();
	hw_sample_wait_phacurr_offset(&motor->samples);
	if (hw_sample_check_phacurr_offset(&motor->samples)){
		motor_stop(motor);
		return false;
	}
    return true;
}

bool motor_stop(motor_t *motor) {
    if (!motor->b_start) {
        return true;
    }
	sys_debug("motor stop\n");
    u32 mask = cpu_enter_critical();
    pwm_stop_output();
    cpu_exit_critical(mask);
    cpu_udelay(100); /* give the time for pending adc irq finished*/
    mask = cpu_enter_critical();
    contrl_stop(controller(motor));
	motor->b_start = false;
    cpu_exit_critical(mask);
    return true;
}


static void motor_force_timeout(os_timer_t *timer) {
	motor_t *motor = container_of(timer, motor_t, force_timer);
	float hz = decode_float(motor->force_timer.priv);
	sys_debug("manual vel = %f\n", hz);
	contrl_set_manual_vel(&motor->controller, hz);
}

bool motor_force_foc_angle(motor_t *motor, float vd, float manu_vel) {
	if (vd == 0) {
		os_timer_cancel(&motor->force_timer);
		contrl_set_target_vdq(&motor->controller, 0, 0);
		os_delay_ms(110);
		motor_stop(motor);
		return true;
	}
	if (motor->b_start) {
		if ((motor->controller.manual_angle_step) != 0 && (motor->controller.mode_req == CTRL_MODE_OPEN)) {
			return true;
		}
		contrl_request_mode(&motor->controller, CTRL_MODE_OPEN);
	}else if (!motor_start(motor, CTRL_MODE_OPEN)){
		return false;
	}
	u32 mask = cpu_enter_critical();
	contrl_set_manual_vel(&motor->controller, 0);
	contrl_set_foc_angle(&motor->controller, true, 0);
	contrl_set_target_vdq(&motor->controller, vd, 0);
	cpu_exit_critical(mask);
	sys_debug("manu hz %f\n", (manu_vel));
	if (manu_vel != 0) {
		encode_float(motor->force_timer.priv, rpm_2_w_hz(manu_vel, mc_conf()->m.poles));
		os_timer_post(&motor->force_timer, 200); //give 200ms to align D
	}
	return true;
}


__STATIC_INLINE void motor_update_samples(motor_t *motor) {
	motor_update_encoder(motor);
	hw_sample_get_phacurr(&motor->samples, motor->controller.phase_curr);
	motor->controller.encoder_angle = motor_get_encoder_angle(motor);    
    motor->controller.encoder_velocity = motor_get_encoder_velocity(motor);
	motor->controller.vbus_vol_sampled = hw_sample_get_vbus_vol(&motor->samples);
	motor->controller.vbus_curr_sampled = hw_sample_get_vbus_curr(&motor->samples);
}

__STATIC_INLINE bool motor_check_over_current(motor_t *motor) {
	float limit = mc_conf()->b.phase_curr_max * 1.5f;
	if ((_abs(motor->controller.phase_curr[0]) > limit) || (_abs(motor->controller.phase_curr[1]) > limit) ||
		(_abs(motor->controller.phase_curr[2]) > limit)){
		motor_stop(motor);
		return true;
	}
	return false;
}


void timer_up_irq_handler(motor_t *motor){

}

void mc_fault_irq_handler(motor_t *motor) {

}

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void motor_adc_irq_handler(motor_t *motor) {
	static u32 ticks;

	motor->adc_count[motor->adc_cnt] = pwm_count();
	motor->adc_cnt = (motor->adc_cnt + 1) % 2;

    motor->delta_ticks = cpu_ticks_rel(ticks);
    ticks = cpu_ticks_abs();

	controller_t *ctrl = controller(motor);
	foc_t *foc = &ctrl->foc;
	if (hw_sample_calc_phacurr_offset(&motor->samples)) {
		return;
	}
	motor_update_samples(motor);
	if (!motor->b_start || motor_check_over_current(motor)) {
		return;
	}
	contrl_get_phase_duty(ctrl);

	u16 ta = foc->duty_norm[0] * mc_conf()->pwm_half_period;
    u16 tb = foc->duty_norm[1] * mc_conf()->pwm_half_period;
    u16 tc = foc->duty_norm[2] * mc_conf()->pwm_half_period;

    pwm_update_duty(ta, tb, tc);

    motor->foc_bench = cpu_ticks_rel(ticks);
}

/*slow的部分处理，比如速度环，状态机*/
void mc_sched_irq_handler(motor_t *motor) {
	u32 ticks = cpu_ticks_abs();
	motor->mode_running = contrl_update_mode(controller(motor));
	if (motor->mode_running == CTRL_MODE_TORQUE) {
		contrl_set_target_torque(controller(motor), motor->torque_command);
	}else if (motor->mode_running == CTRL_MODE_VELOCITY) {
		contrl_set_target_velocity(controller(motor), motor->velocity_command);
	}
	contrl_update_target(controller(motor));
    motor->slow_bench = cpu_ticks_rel(ticks);
}
